#include "Mpu6050.h"
#include <math.h>
#include "task.h"
#include "cmsis_os2.h"
#define MPU_6050_ADDRESS 0xd0

extern I2C_HandleTypeDef  hi2c1;
extern osThreadId_t  mpu6050TaskHandle;

static void send_1_byte_command(uint8_t register_addr,uint8_t value){
    
    HAL_I2C_Mem_Write(
    		&hi2c1,
    		MPU_6050_ADDRESS,
    		register_addr,I2C_MEMADD_SIZE_8BIT,
			&value,
			1,
			100
	);

}

void Delay()
{

	osDelay(50);
 /*   int i=0,j=0;
    //在初始化之前要延时一段时间，若没有延时，则断电后再上电数据可能会出错
    for(i=0;i<1000;i++)
    {
        for(j=0;j<1000;j++)
        {
            ;
        }
    }*/
}

#define PWR_MANAGEMENT_REG_1 0x6b

#define PWR_MANAGEMENT_REG_2 0x6c

#define CONFIG_REG 0x1a

#define CYRO_CONFIG_REG 0x1b

#define ACCEL_CONFIG_REG 0x1c



void init_mpu6050(void){
    


   // 示例：粗略延时
   Delay();
     
    // 复位 mpu6050
    send_1_byte_command(PWR_MANAGEMENT_REG_1,0x80);
    
    // 示例：粗略延时
   Delay();
    
    // 退出睡眠模式
    send_1_byte_command(PWR_MANAGEMENT_REG_1,0x00);
    
    // 使用陀螺仪x轴时钟
    send_1_byte_command(PWR_MANAGEMENT_REG_1,0x01);
    
    
     send_1_byte_command(PWR_MANAGEMENT_REG_2,0x00);
    
    // 配置陀螺仪和加速度计 的低通滤波器
    // 加速度计 带宽 184Hz
    // 陀螺仪 带宽 188Hz
    send_1_byte_command(CONFIG_REG,0x01);
    
    
    /* 一旦配置了低通滤波器，则陀螺仪输出率为 1kHz
     暂时不配置分频
    where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or 7), 
    and 1kHz when the DLPF is enabled (see Register 26).
    
    Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
    
    send_1_byte_command(0x19,0x01); // 设置采样率分频
    */
    send_1_byte_command(0x19,0x01); // 设置采样率分频

     
    // 配置陀螺仪量程  +- 1000°/s
    send_1_byte_command(CYRO_CONFIG_REG,0x10);

    // 配置加速度计 量程  +- 2g
    send_1_byte_command(ACCEL_CONFIG_REG,0x00);
    
}
/*
uint8_t read_who_am_I(void){
    
    uint8_t result;
    
    soft_get_data(MPU_6050_ADDRESS,0x75,&result,1);
    
    return (result & 0x7e)>>1;
}

uint16_t read_accel_x(void){
    
    uint8_t result[2];
    
    soft_get_data(MPU_6050_ADDRESS,0x43,result,2);
    
    
    
    return result[0]<<8 | result[1];
}*/


#define alpha    0.95238 //0.9804

uint8_t raw_sensor_data[14];

Euler_angles euler_angles = {0,0,0};




/**
 * 通过dma和dma传输完成中断 发起对mpu6050 加速度计和陀螺仪寄存器的读取操作
 */
void init_read_accel_gyro(){


    /*
	HAL_I2C_Mem_Read(hi2c1,
			MPU_6050_ADDRESS,
			0x3b,
			I2C_MEMADD_SIZE_8BIT ,
			raw_sensor_data,
			14,
			1000
			);
*/

    if(HAL_I2C_Mem_Read_DMA(&hi2c1,
			MPU_6050_ADDRESS,
			0x3b,
			I2C_MEMADD_SIZE_8BIT ,
			raw_sensor_data,
			14
    )!=HAL_OK){
    	//static char tips[]= "init i2c read mpu6050 through dma failed\r\n";
    	//HAL_UART_Transmit(huart,tips,sizeof(tips),100);
    }

/*
	HAL_I2C_Mem_Read_IT(hi2c1,
			MPU_6050_ADDRESS,
			0x3b,
			I2C_MEMADD_SIZE_8BIT ,
			raw_sensor_data,
			14);
*/

}

void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *hi2c){



	if (hi2c->Instance == I2C1) {

		/* xHigherPriorityTaskWoken must be initialised to pdFALSE. If calling
		       vTaskNotifyGiveFromISR() unblocks the handling task, and the priority of
		       the handling task is higher than the priority of the currently running task,
		       then xHigherPriorityTaskWoken will automatically get set to pdTRUE. */
		BaseType_t  xHigherPriorityTaskWoken = pdFALSE;

		// 传入 pdFALSE 以使用二进制信号量模式
		// 详情请看 task.h 中 vTaskNotifyGiveFromISR 的方法参数说明
		vTaskNotifyGiveFromISR( mpu6050TaskHandle,  &xHigherPriorityTaskWoken );


	}

}

Euler_angles calculate_euler_angles(void){

	// 拼接高8位和低8位的数据为init16_t
	int16_t acc_raw_x =  raw_sensor_data[0] << 8 | raw_sensor_data[1] ;
	int16_t acc_raw_y =  raw_sensor_data[2] << 8 | raw_sensor_data[3] ;
	int16_t acc_raw_z =  raw_sensor_data[4] << 8 | raw_sensor_data[5] ;




	// 根据之前配置的加速度计的量程，把int16_t 归一化到[-2,2]g 内
	//accel_output[j] = temp * 2.0 /0x7fff;


	float pitch_acc = atan2(acc_raw_x,acc_raw_z) * 180 /3.1415926;
	float roll_acc = atan2(acc_raw_y,acc_raw_z) * 180 /3.1415926;





	// 拼接i2c总线上的高8位和低8位数据为一个int16_t
	int16_t gyro_raw_x = raw_sensor_data[8]<<8 | raw_sensor_data[9] ;
	int16_t gyro_raw_y = raw_sensor_data[10]<<8 | raw_sensor_data[11] ;
	int16_t gyro_raw_z = raw_sensor_data[12]<<8 | raw_sensor_data[13] ;

	float gyro_x=0,gyro_y=0,gyro_z=0;
	// 把陀螺仪数据转换为 角速度 (度/秒)
	gyro_x = gyro_raw_x * 1000 / 0x7fff * 0.01;
	gyro_y = gyro_raw_y * 1000 / 0x7fff * 0.01;
	gyro_z = gyro_raw_z * 1000 / 0x7fff * 0.01;


	euler_angles.pitch = alpha * (euler_angles.pitch + gyro_y) + (1-alpha) * pitch_acc;
	euler_angles.roll = alpha * (euler_angles.roll + gyro_x) + (1-alpha) * roll_acc;

	return euler_angles;
}

Euler_angles * get_euler_angles(void){

	return &euler_angles;
}



